/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  */
  
#include "main.h"
#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#define SERIAL_BANDRATE 115200
int fdUart;
int ser_fd,conn;
char filename[40];
struct timeval tv;
 
FILE *f_IMU=NULL,*f_GNSS=NULL;
static const double numpi = 3.14159265358979324;
static const double G0 = 9.7864;
/**
 * @brief GNGGA_Set
 * decode NMEA format data into GNGGA structure
 * @param NMEA string starting with "$GNGGA"
 * @retval GNGGA_Data
 */
GNGGA_Data GNGGA_Set(char *buff)
{
	GNGGA_Data ans;
	char *header=strstr(buff,"$GNGGA");
	double RAWutc,RAWlat,RAWlongt;
	char dlat,dlongt;
	memset(&ans,0,sizeof(GNGGA_Data));
	sscanf(header,"$GNGGA,%lf,%lf,%c,%lf,%c,%d,%d,%lf,%lf,M,%lf,M",\
	&RAWutc,&RAWlat,&dlat,&RAWlongt,&dlongt,&ans.GNSS_Status,\
	&ans.SatelliteNum,&ans.HDOP,&ans.Altitude,&ans.RaletiveHight);
	ans.UTC_Hour=(int)RAWutc/10000;
	ans.UTC_Minute=(int)RAWutc%10000/100;
	ans.UTC_Second=(int)RAWutc%100;
	ans.Latitude+=(int)RAWlat/100;
	ans.Latitude+=(RAWlat-((int)RAWlat/100)*100)/60.0;
	ans.Longitude+=(int)RAWlongt/100;
	ans.Longitude+=(RAWlongt-((int)RAWlongt/100)*100)/60.0;
	ans.LatDir=dlat;
	ans.LongDir=dlongt;
	return ans;
}

/**
 * @brief Initialize the uart
 * @retval GNGGA_Data
 */
int initUart(void)
{
	
	if(wiringPiSetup()<0)
		return 1;
	if((fdUart=serialOpen("/dev/ttyAMA0",SERIAL_BANDRATE))<0)
		return 2;
		/*block configuration */
	//int flags = fcntl(fd, F_SETFL, O_NONBLOCK);
	//fcntl(fd, F_SETFL, flags | O_NONBLOCK);
	printf("serial start band rate:%d\n",SERIAL_BANDRATE);	
	return 0;
}

/**
 * @brief GNGGA_Set
 * decode NMEA format data into GNGGA structure
 * @param NMEA string starting with "$GNGGA"
 * @retval GNGGA_Data
 */
void  WebSocketSendData(char* buff,int size)
{
	static  frame_head head;
	head.fin=1;
	head.opcode=1;
	head.mask=0;
	head.payload_length=size;
	send_frame_head(conn,&head);
	write(conn,buff,size);  
}
static int a=0,GPointer=0,APointer=0,GNSSPointer=0;
static BMI160_FloatData GBuff[1024],Abuff[1024];
double anslng=121,anslat=31,x=0,y=0;
GNGGA_Data GNSSBuff[10];
PI_THREAD(threadFile)
{
	static EKF_RawData ekf;
	static int firstcall=1;
	while(1)
	{
		piLock(0);
		piLock(1);
		int j=0;
		for(int i=0;i<APointer;i++)
		{
			ekf.TimeStamp=GBuff[i].time;
			ekf.gx_rps=GBuff[i].x; 
			ekf.gy_rps=GBuff[i].y;
			ekf.gz_rps=GBuff[i].z;
			ekf.fx_mps2=Abuff[i].x;
			ekf.fy_mps2=Abuff[i].y;
			ekf.fz_mps2=Abuff[i].z;
			ekf.isGPSUpdate=0;
			if(GNSSPointer)
			{
				j=GNSSPointer;
					ekf.isGPSUpdate=1;
					ekf.GNGGA_latitude=GNSSBuff[j].Latitude;
					ekf.GNGGA_longtitude=GNSSBuff[j].Longitude;
					ekf.GNGGA_height=GNSSBuff[j].Altitude;
				if(GNSSPointer>0)GNSSPointer--;
			}
			char STR[50];
				EKF_Update(&ekf);
				sprintf(STR,"%.6lf %.6lf ",ekf.EKF_POS_E,ekf.EKF_POS_N);
				printf("[Thread EKF]%s\n",STR);
			
		}
		
		GNSSBuff[0].Longitude=121;
		GNSSBuff[0].Latitude=31;
		
		
		//WebSocketSendData(STR,strlen(STR));
		GNSSPointer=0;
		GPointer=0;
		APointer=0;
		piUnlock(1);
		piUnlock(0);
		delay(300);
	}
}

PI_THREAD(piThreadGNSS)
{
	char buff[1024];
	while(1)
	{
		if(serialDataAvail(fdUart))
		{
			int temp = serialGetchar(fdUart),count=0;
			if(temp=='$')
			{
				while(temp!='*')
				{
					buff[count++]=temp;
					temp=serialGetchar(fdUart);
				}
				
				buff[count]='*';
				buff[count+1]='\0';
				if(strstr(buff,"$GNGGA")!=NULL)
				{
					gettimeofday(&tv, NULL);
					fprintf(f_GNSS,"[%d.%06d]%s\n",\
						tv.tv_sec,tv.tv_usec,buff);
					fflush_unlocked(f_GNSS);	
				}
			}
		}	
	}
}

PI_THREAD(piThreadIMU)
{
	while(1)
	{
		BMI160_Data accel,gyro;
		BMI160_FloatData at,gt;
		gyro=BMI160_GetGyro();	
		gt.x=(double)gyro.x*2000/32768*numpi/180.0;
		gt.y=(double)gyro.y*2000/32768*numpi/180.0;
		gt.z=(double)gyro.z*2000/32768*numpi/180.0;
		accel=BMI160_GetAccel();
		at.x=(double)accel.x*2*G0/32768.0;
		at.y=(double)accel.y*2*G0/32768.0;
		at.z=(double)accel.z*2*G0/32768.0;
		at.time=millis();
		gettimeofday(&tv, NULL);
		fprintf(f_IMU,"[%d.%06d]%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n",\
			tv.tv_sec,tv.tv_usec,\
			gt.x,gt.y,gt.z,at.x,at.y,at.z);		
		fflush_unlocked(f_IMU);
	}
}
/**
 * @brief main
 * @retval int
 */
int main()
{	
	char str[40];
	//EKF_RawData ekf;
	initBMI160();
	initUart();
	gettimeofday(&tv, NULL);
	sprintf(str,"Data/IMU_%d.txt",tv.tv_sec);
	f_IMU = fopen(str,"wr+");
	
	gettimeofday(&tv, NULL);
	sprintf(str,"Data/GNSS_%d.txt",tv.tv_sec);
	f_GNSS = fopen(str,"wr+");
		
	if(piThreadCreate(piThreadGNSS)==0)
		printf("successfully created thread:GNSS\n");
	else
		printf("failed to creat thread\n");
		
	if(piThreadCreate(piThreadIMU)==0)
		printf("successfully created thread:IMU\n");
	else
		printf("failed to creat thread\n");
	
	
	
	while(1);
	serialClose(fdUart);
	return 0;
}
